#include "SystemDataUtils.h"
#include "Constraint.h"
#include "MultibodySystem.h"
#include "List.h"

using namespace RSIM;

///////////////////////////////////////////////////////////////////////////

bool SystemDataUtils::areConstraintsCoupled(const Constraint *Cons1,
                                            const Constraint *Cons2){
        // Parameters for Constraint 1
        const MultibodySystem *parent_1 = Cons1->getParentSystem();
        const int& parent_nd_1 = Cons1->getParentNodeId();
        const MultibodySystem *child_1 = Cons1->getChildSystem();
        const int& child_nd_1 = Cons1->getChildNodeId();
        const int& common_parent_1 = Cons1->getCommonParentId();
        
        
        const MultibodySystem *parent_2 = Cons2->getParentSystem();
        const int& parent_nd_2 = Cons2->getParentNodeId();
        const MultibodySystem *child_2 = Cons2->getChildSystem();
        const int& child_nd_2 = Cons2->getChildNodeId();
        const int& common_parent_2 = Cons2->getCommonParentId();
        
        bool ret=false;                
        
        // Case 1: (parent_1 == parent_2)
        if(parent_1==parent_2){
                ret = areConstraintBranchesOverlapping(parent_1,
                                                parent_nd_1,
                                                common_parent_1,
                                                parent_nd_2,
                                                common_parent_2);

                // No point in checking for other cases. Since constraints are coupled
                if(ret){return true;}
        }
        
        // Case 2: (parent_1 == child_2)
        if(parent_1 == child_2){
                ret = areConstraintBranchesOverlapping( parent_1,
                                                        parent_nd_1,
                                                        common_parent_1,
                                                        child_nd_2,
                                                        common_parent_2);
                if(ret){return true;}
        }
        
        // Case 3: (child_1 == parent_2)
        if(child_1 == parent_2){
                ret = areConstraintBranchesOverlapping(	child_1,
                                                        child_nd_1,
                                                        common_parent_1,
                                                        parent_nd_2,
                                                        common_parent_2);
                if(ret){return true;}
        }
        
        // Case 4: (child_1 == child_2)
        // If both the children are ground, then no need to do any checks here.
        if((child_1==child_2) && (child_1!=NULL)){
                ret = areConstraintBranchesOverlapping(child_1,
                                                child_nd_1,
                                                common_parent_1,
                                                child_nd_2,
                                                common_parent_2);
                if(ret){return true;}
        }
        // This is the case when none of the systems between the two constraints
        // are common. In this case, the constraints are certainly decoupled.
        return false;
}

///////////////////////////////////////////////////////////////////////////

void SystemDataUtils::sortCoupledConstraintList_1(const int& nConstraints,
                                                const List<List<int> >& cl,
                                                List<List<int> >& ccouple)const{
        List <int> coupled_constraints;
        for(int i=0;i<cl.getLength();++i){
                recursiveSortCCList(cl,cl[i][0],ccouple,coupled_constraints);
                if(coupled_constraints.getLength()){
                        ccouple.addNode(coupled_constraints);
                        coupled_constraints.clear();
                }
                recursiveSortCCList(cl,cl[i][1],ccouple,coupled_constraints);
                if(coupled_constraints.getLength()){
                        ccouple.addNode(coupled_constraints);
                        coupled_constraints.clear();
                }
        }
        
        // If any constraints in the system are not coupled with others,
        // they are added in teh following loop
        for(int i=0;i<nConstraints;++i){
                int id1,id2;
                this->findElement(ccouple,i,id1,id2);
                if(id1+1)
                        continue;
                else{
                        List <int> tmp;
                        tmp.addNode(i);
                        ccouple.addNode(tmp);
                        tmp.clear();
                }
        }
        ccouple.finalize();
        for(int i=0;i<ccouple.getLength();++i)
                ccouple[i].finalize();        
}

///////////////////////////////////////////////////////////////////////////

void SystemDataUtils::sortCoupledConstraintList(const int& nConstraints,
                                                const List<List<int> >& cl,
                                                List<List<int> >& ccouple)const{
        int id1,id2,id3,id4;
        List <int> c_master;
        for(int i=0;i<cl.getLength();++i){
                
                // if elements cl[i][0] or 
                this->findElement(ccouple,cl[i][0],id1,id2);
                this->findElement(ccouple,cl[i][1],id3,id4);
                                
                if( (id1+1) || (id3+1)){                        
                // this case implies atleast cl[i][0] or cl[i][1] or both are already
                // added
                        
                        // if both are added, make sure they are at the same index
                        if((id1+1) && (id3+1)){
                                assert(id1==id3);
                                continue;
                        }
                                                
                        if(!(id1+1)){
                                // if cl[i][0] is not added
                                ccouple[id3].addNode(cl[i][0]);
                                c_master.addNode(cl[i][0]);
                        }
                        else{
                                // if cl[i][1] is not added
                                ccouple[id1].addNode(cl[i][1]);
                                c_master.addNode(cl[i][1]);
                        }                 
                }
                else{
                // if both cl[i][0] and cl[i][1]  are not present
                        ccouple.addNode(cl[i]);
                        c_master.addNode(cl[i][0]);
                        c_master.addNode(cl[i][1]);
                }
        }        
        
        // finally check if there are any constraints which are not coupled at all.
        // They are added as separate rows in ccouple.
        for(int i=0;i<nConstraints;++i){
                id1 = c_master.findElement(i);
                if(id1+1)
                        continue;
                else{
                        List <int> tmp;
                        tmp.addNode(i);
                        ccouple.addNode(tmp);
                }
        }
        
        // finalize list.
        ccouple.finalize();
        for(int i=0;i<ccouple.getLength();++i)
                ccouple[i].finalize();                
}

///////////////////////////////////////////////////////////////////////////

bool SystemDataUtils::areConstraintBranchesOverlapping(	const MultibodySystem *system,
                                                        const int& parent_nd_1,
                                                        const int& common_parent_1,
                                                        const int& parent_nd_2,
                                                        const int& common_parent_2
                                ){
        const List <List<int> >& cMap = system->cMap_;
                
        // if there are two constraints on teh same system wiht respect to
        // ground, they are certainly coupled
        if(common_parent_1 == -1 && common_parent_2 == -1){
                return true;
        }
        
        int id1 = cMap[parent_nd_1].getLength()-1;
        int id2 = cMap[parent_nd_2].getLength()-1;
        int id[2],branch_id[2];		
        
        // Sort the branches in decreasing length.
        if(id1>=id2){
                id[0] = id1; id[1] = id2;
                branch_id[0] = parent_nd_1; branch_id[1] = parent_nd_2;
        }
        else{
                id[0] = id2; id[1] = id1;
                branch_id[0] = parent_nd_2; branch_id[1] = parent_nd_1;
        }
        
        // next_child is the body immediately after both the common_parent's are 
        // encountered for the longer chain. 
        int next_child=-2;
        
        bool flag1, flag2;
        flag1 = false; flag2 = false;
                
        // go through the longer branch first
        for(int ii=id[0];ii>=0;--ii){
                if(flag1 && flag2){
                        next_child = cMap[branch_id[0]][ii];
                        break;
                }
                if(cMap[branch_id[0]][ii] == common_parent_1 or common_parent_1 == -1){
                        flag1 = true;
                }
                if(cMap[branch_id[0]][ii] == common_parent_2 or common_parent_2 == -1){
                        flag2 = true;
                }
        }
                
        if(flag1 && flag2){		
                flag1 = false; flag2 = false;		
                // Shorter branch next
                for(int ii=id[1];ii>=0;--ii){
                        if(flag1 && flag2){
                                if(next_child == cMap[branch_id[1]][ii]){
                                        // Atleast one body is common to both the constraints.
                                        // Hence, return true for isCoupled.
                                        return true;					
                                }
                                else{
                                        return false;
                                }
                        }
                        
                        if(cMap[branch_id[1]][ii] == common_parent_1 or common_parent_1 == -1){
                                flag1 = true;
                        }
                        if(cMap[branch_id[1]][ii] == common_parent_2 or common_parent_2 == -1){
                                flag2 = true;
                        }
                }
        }//if(flag1 and flag2)

        // if both the common_parent's are not enountered
        // on parsing the longer branch, then there is no possibility
        // of coupling between these branches of the constraints.
        return false;
        
}

///////////////////////////////////////////////////////////////////////////

int SystemDataUtils::calcCommonParentForConstraint(const List<MultibodySystem*>& system,
                                                Constraint *cPtr){	
                
        const MultibodySystem *parent = cPtr->getParentSystem(); 
        const MultibodySystem *child = cPtr->getChildSystem();                         
        
        const int& parent_node = cPtr->getParentNodeId();
        const int& child_node = cPtr->getChildNodeId();         
        
        // This is the only variable of the class constraint which is updated
        // in this function.
        List <int>& Parent_path = cPtr->wgetParentPathList();
        List <int>& Child_path = cPtr->wgetChildPathList();
                
        // Obtain the connectivity map for the multibody system
        const List <List<int> >& cMap_parent = parent->cMap_;        
        const List <int>& parent_map = cMap_parent[parent_node];        
        
        int plen = parent_map.getLength()-1;        
        
        // Case when both the constraints are on different multibody system
        // or if the child body is ground, then common parent is ground body 
        // denoted by -1;
        if(parent!=child || child == NULL){
                
                // Path from parent body to ground
                for(int i=0;i<=plen;Parent_path.addNode(parent_map[i++]));
                Parent_path.addNode(-1);
                Parent_path.finalize();
                
                // if child == -1, nothing needs to be done
                if(child!=NULL){
                        const List <List<int> >& cMap_child = child->cMap_;
                        const List <int>& child_map = cMap_child[child_node];
                        int clen = child_map.getLength()-1;
                        
                        for(int i=0;i<=clen;Child_path.addNode(child_map[i++]));
                        
                        Child_path.addNode(-1);
                        Child_path.finalize();
                }                                
                
                return -1;
        }
        else{	// if parent == child		
                
                const List <List<int> >& cMap_child = child->cMap_;
                const List <int>& child_map = cMap_child[child_node];
                int clen = child_map.getLength()-1;
                
                // to be returned
                int common_parent;
                
                int pid,cid;
                const int PLEN = plen;
                const int CLEN = clen;
                
                if(PLEN<=CLEN){                        
                        for(int ii=plen;ii>=0;--ii){
                                if(parent_map[ii]!=child_map[clen--]){                                                              
                                        break;
                                }
                                common_parent = parent_map[ii];                                
                                pid = ii;
                                cid = CLEN - PLEN + ii;
                        }
                }
                else{
                        for(int ii=clen;ii>=0;--ii){
                                if(parent_map[plen--]!=child_map[ii]){                                        
                                        break;
                                }
                                common_parent = child_map[ii];
                                pid = PLEN - CLEN + ii;
                                cid = ii;
                        }
                }
                                
                for(int i=0;i<=pid;Parent_path.addNode(parent_map[i++]));
                for(int i=0;i<=cid;Child_path.addNode(child_map[i++]));
                Parent_path.finalize();
                Child_path.finalize();
                
                return common_parent;
        }	
}

///////////////////////////////////////////////////////////////////////////

bool SystemDataUtils::q_check(	const List<List<int> >& ccouple,
                                const int& i,
                                const int& j,
                                int *i_index){

        bool flag1, flag2;
        flag1 = false;
        flag2 = false;
        int tmp[2];
        tmp[0] = -1;
        tmp[1] = -1;
        
        for(int iloop = 0;iloop<ccouple.getLength();++iloop){
                for(int jloop=0; jloop<ccouple[iloop].getLength(); ++jloop){
                        const int& tmp = ccouple[iloop][jloop];
                        if(tmp == i)
                                flag1 = true;
                        if(tmp == j)
                                flag2 = true;
                }
                if(flag1){
                        tmp[0] = iloop;
                        flag1 = false;
                }
                if(flag2){
                        tmp[1] = iloop;
                        flag2 = false;
                }
        }

        if((tmp[0]+1) && (tmp[1]+1)){
                i_index[0] = -1;
                return false;
        }
        if(tmp[0]+1){
                i_index[0] = tmp[0];
                i_index[1] = j;
                return true;
        }
        if(tmp[1]+1){
                i_index[0] = tmp[1];
                i_index[1] = i;
                return true;
        }
        // reset the value of i_index before returning
        i_index[0]=0;
        return false;
}

///////////////////////////////////////////////////////////////////////////

void SystemDataUtils::findElement(const List<List<int> >& list, 
                                const int& element,
                                int& index1,
                                int& index2)const{
        index1 = -1;
        index2 = -1;
        for(int i=0;i<list.getLength();++i){
                List <int> tmp = list(i);
                for(int j=0;j<tmp.getLength();++j){
                        if(tmp(j)==element){
                                index1 = i;
                                index2 = j;
                                return;
                        }
                }
        }
}

///////////////////////////////////////////////////////////////////////////

void SystemDataUtils::recursiveSortCCList(const List<List<int> >& list,
                                        const int& element,
                                        const List<List<int> >& ccouple,
                                        List <int>& coupled_constraints)const{
        int id1,id2;
        bool is_element_added = false;
        this->findElement(ccouple,element,id1,id2);
        if(id1+1)
                return;      
        
        id1 = coupled_constraints.findElement(element);
        if(id1+1)
                is_element_added = true;
        
        List <int> tmp;
        for(int i=0;i<list.getLength();++i){
                if(list[i][0] == element){
                        if(!is_element_added){
                                coupled_constraints.addNode(element);
                                is_element_added = true;
                        }
                        
                        id1 = coupled_constraints.findElement(list[i][1]);
                        if(id1+1)
                                continue;
                        else{
                                coupled_constraints.addNode(list[i][1]);
                                tmp.addNode(list[i][1]);
                        }                                
                }
                else if(list[i][1]==element){
                        if(!is_element_added){
                                coupled_constraints.addNode(element);
                                is_element_added = true;
                        }
                        
                        id1 = coupled_constraints.findElement(list[i][0]);
                        if(id1+1)
                                continue;
                        else{
                                coupled_constraints.addNode(list[i][0]);
                                tmp.addNode(list[i][0]);
                        }                        
                }
        }
        
        for(int i=0;i<tmp.getLength();++i)
                recursiveSortCCList(list,tmp[i],ccouple,coupled_constraints);
}

///////////////////////////////////////////////////////////////////////////